#include <localization/dynamics_model.h>
#include <localization/localization.h>

DynamicsModel::DynamicsModel(PlayerCc::Position2dProxy *position2d_proxy) :
        position2d_proxy_(position2d_proxy) {

  last_reading_.px = position2d_proxy_->GetXPos();
  last_reading_.py = position2d_proxy_->GetYPos();
  last_reading_.pa = position2d_proxy_->GetYaw();

  G_ = gsl_matrix_alloc(3, 3);
  V_ = gsl_matrix_alloc(3, 2);
  M_ = gsl_matrix_alloc(2, 2);
  aux_3x3_ = gsl_matrix_alloc(3, 3);
  aux_3x2_ = gsl_matrix_alloc(3, 2);

  gsl_matrix_set_zero(G_);
  gsl_matrix_set_zero(V_);
  gsl_matrix_set_zero(M_);
  gsl_matrix_set_zero(aux_3x3_);
  gsl_matrix_set_zero(aux_3x2_);

  gsl_matrix_set(G_, 0, 0, 1.0);
  gsl_matrix_set(G_, 1, 1, 1.0);
  gsl_matrix_set(G_, 2, 2, 1.0);

  gsl_matrix_set(V_, 2, 1, 1.0);
}

DynamicsModel::~DynamicsModel() {
  gsl_matrix_free(G_);
  gsl_matrix_free(V_);
  gsl_matrix_free(M_);
  gsl_matrix_free(aux_3x3_);
  gsl_matrix_free(aux_3x2_);
}

void DynamicsModel::UpdateBelief(player_pose2d *mean, gsl_matrix *covariance) {
  // Obtains the odometry data
  player_pose2d new_reading = { position2d_proxy_->GetXPos(),
          position2d_proxy_->GetYPos(), position2d_proxy_->GetYaw() };

  double delta_x = new_reading.px - last_reading_.px;
  double delta_y = new_reading.py - last_reading_.py;
  double delta_trans = sqrt(delta_x * delta_x + delta_y * delta_y);
  double delta_theta = new_reading.pa - last_reading_.pa;
  double delta_rot1 = Localization::AdjustAngle(atan2(delta_y, delta_x) - last_reading_.pa);
  double theta_aux = mean->pa + delta_rot1;

  last_reading_ = new_reading;

  // Calculates the uncertainty component due to the previous estimate's uncertainty
  gsl_matrix_set(G_, 0, 2, -delta_trans * sin(theta_aux));
  gsl_matrix_set(G_, 1, 2, delta_trans * cos(theta_aux));

  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, G_, covariance, 0.0,
          aux_3x3_);
  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, aux_3x3_, G_, 0.0,
          covariance);

  // Calculates the uncertainty component due to motion noise
  double variance_delta_trans = delta_trans * kCovarTransTrans;
  variance_delta_trans *= variance_delta_trans;

  double variance_delta_theta = (delta_theta * delta_theta * kCovarThetaTheta * kCovarThetaTheta);
  variance_delta_theta += delta_trans * delta_trans * kCovarTransTheta * kCovarTransTheta;

  gsl_matrix_set(M_, 0, 0, variance_delta_trans);
  gsl_matrix_set(M_, 1, 1, variance_delta_theta);

  gsl_matrix_set(V_, 0, 0, cos(theta_aux));
  gsl_matrix_set(V_, 1, 0, sin(theta_aux));

  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, V_, M_, 0.0, aux_3x2_);
  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, aux_3x2_, V_, 1.0,
          covariance);

  // Updates the mean. The previous line updates the covariance
  mean->px += delta_trans * cos(theta_aux);
  mean->py += delta_trans * sin(theta_aux);
  mean->pa = Localization::AdjustAngle(mean->pa + delta_theta);
}

